/**
    Copyright 2015 Table Rider
    By: Richard Solti
*/

#include "Definitions.h"
#include "Motor.h"
#ifndef CONTROLLERMOTOR_H
#define CONTROLLERMOTOR_H


/*The Controller independently controls both motors. The controller is dependant on the two encoders because:
Moving to a position is separated into two steps: Turning and Rolling.
The turning steps obviously turns the robot and the rolling step, which is not as obvious, makes the robot go forward/backward.
When execute is called, the turn step is divided into 2 values, one for each encoder. These two values is a theoretical
value for how much the motors have to rotate before they get to their destination.

To avoid spinning out, the maxTurn and maxAcceleration variables limit how fast the robot can start off.*/

class ControllerMotor
{
public:
    ControllerMotor(Motor *, Motor *);
    void init(long, long);
    void update();
    int done();
private:
    void positionLoop();
    void speedLoop();
    void currentLoop();
    void convertAD();
    void writeGrafToFile();
    Motor *left, *right;
    int victory, treshold;          //Regulation stops when dPos and current position differs less than treshold for both motors
    long     oldPosL, oldPosR,       //Previous encoder values
            dPosL, dPosR;           //Desired encoder values
    double  dSpeedL, dSpeedR,       //Desired speed
            dCurrL, dCurrR,         //Desired current/duty
            speedSumL, speedSumR,   //Integrated speed
            clampVal, acceleration;
    double limiter, syncer;
    double posP, speedP, speedI, currP;
    long cLoop, sLoop, pLoop;//Numbers are in milliseconds

};

#endif // CONTROLLERMOTOR_H
